#include "Sensor.h"

AS5600::AS5600(int sda, int scl, uint32_t frequency)
{
    Wire.begin(sda, scl, frequency);
    this->device_address = 0x36;
    this->high_address = 0x0c;
    this->low_address = 0x0d;
    this->angle_buffer = 0;
    this->init_angle = 0;
    this->read_angle_loop_flag = false;
    this->read_angle_handlep = NULL;
    this->mutex_handle = xSemaphoreCreateMutex();
}

uint8_t AS5600::_read_register(uint8_t device_address, uint8_t register_address)
{
    uint8_t data = 0;
    Wire.beginTransmission(device_address);
    Wire.write(register_address);
    Wire.endTransmission(false);
    Wire.requestFrom(device_address, 1);
    if (Wire.available())
    {
        data = Wire.read();
    }
    return data;
}

void AS5600::read_angle()
{
    uint8_t angle_high = _read_register(device_address, high_address);
    uint8_t angle_low = _read_register(device_address, low_address);

    uint16_t angle = (angle_high << 8) | angle_low;
    angle &= 0x0fff;

    xSemaphoreTake(this->mutex_handle, 100);
    this->angle_buffer = angle * 2 * PI / 4096.0 - this->init_angle;
    xSemaphoreGive(mutex_handle);
}

void AS5600::_read_angle_loop()
{
    while (this->read_angle_loop_flag)
    {
        this->read_angle();
        Serial.println(this->angle_buffer);
        vTaskDelay(10);
    }
    vTaskDelete(this->read_angle_handlep);
}

void AS5600::read_angle_task_start()
{
    this->read_angle_loop_flag = true;
    this->read_angle_handlep = new TaskHandle_t;

    xTaskCreate(
        AS5600::_read_angle_loop_wrapper,
        "read_angle_loop",
        10000,
        this,
        1,
        this->read_angle_handlep);
}

void AS5600::read_angle_task_end()
{
    this->read_angle_loop_flag = false;
    delete (this->read_angle_handlep);
    this->read_angle_handlep = NULL;
}

double AS5600::get_angle()
{
    xSemaphoreTake(this->mutex_handle, 100);
    double temp = angle_buffer;
    xSemaphoreGive(mutex_handle);
    return temp;
}

void AS5600::set_init_angle()
{
    read_angle();
    this->init_angle = this->angle_buffer;
}